#include <ros/ros.h>
#include <std_msgs/Int8.h>
#include <std_msgs/UInt32.h>
#include <serial/serial.h>
#include <chrono>
#include <thread>
#include <collect_pkg/motor_control_node_plus.h>
#include <fcntl.h>
#include <stdlib.h>
#include <termios.h>
#include <unistd.h>
#include <iostream>
using namespace std;
#define ESC_ASCII_VALUE 0x1b
#define A_ASCII_VALUE 0x41
#define B_ASCII_VALUE 0x42
#define C_ASCII_VALUE 0x43
#define D_ASCII_VALUE 0x44

#define E_ASCII_VALUE 0x45
#define F_ASCII_VALUE 0x46
#define G_ASCII_VALUE 0x47
#define H_ASCII_VALUE 0x48

#define I_ASCII_VALUE 0x49
#define J_ASCII_VALUE 0x4a
#define K_ASCII_VALUE 0x4b
#define L_ASCII_VALUE 0x4c

#define M_ASCII_VALUE 0x4d
#define N_ASCII_VALUE 0x4e
#define O_ASCII_VALUE 0x4f
#define P_ASCII_VALUE 0x50

#define Q_ASCII_VALUE 0x51
#define R_ASCII_VALUE 0x52
#define S_ASCII_VALUE 0x53
#define T_ASCII_VALUE 0x54

#define U_ASCII_VALUE 0x55
#define V_ASCII_VALUE 0x56
#define W_ASCII_VALUE 0x57
#define X_ASCII_VALUE 0x58

#define Y_ASCII_VALUE 0x59
#define Z_ASCII_VALUE 0x5a

int keyValue = -1;

// 获取
int getch()
{
    struct termios oldt, newt;
    int ch;
    tcgetattr(STDIN_FILENO, &oldt);
    newt = oldt;
    newt.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
    ch = getchar();
    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
    return ch;
}

// 函数：获取低八位
int getLowByte(int num)
{
    return num & 0xFF; // 0xFF 是 11111111 (二进制)，表示只保留最低的 8 位
}

// 函数：获取次低八位
int getnext_LowByte(int num)
{
    return (num >> 8) & 0xFF; // 右移 8 位，然后与 0xFF 按位与，保留次低 8 位
}

// 函数：获取次高八位
int getnext_HighByte(int num)
{
    return (num >> 16) & 0xFF; // 右移 8 位，然后与 0xFF 按位与，保留次高 8 位
}

// 函数：获取高八位
int getHighByte(int num)
{
    return (num >> 24) & 0xFF; // 右移 8 位，然后与 0xFF 按位与，保留高 8 位
}

uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed)
{
    uint8_t crc_u = crc ^ crc_seed;

    for (int i = 0; i < 8; ++i)
    {
        crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
    }

    return crc_u;
}

// CRC8校验
uint8_t crc8(const uint8_t *buf, uint8_t len)
{
    uint8_t crc = 0;

    for (int i = 0; i < len; ++i)
    {
        crc = updateCrc8(buf[i], crc);
    }

    return crc;
}

// 串口连接
serial::Serial ser;

void timerCallback(const ros::TimerEvent &)
{
    ROS_INFO("定时器触发了!");
}

// 自动
void task::automatic()
{
    int publish_frequency = 1; // 1000 Hz (1秒发布1000次)
    int publish_duration = 1; // 发布时长为10秒
    int speed = 4000;
    for (int i = 0; i < 10; i++)
    {
        int amplitude = 0.5;
        for (int j = 0; j < 5; j++)
        {
            int phase = -180;
            for (int k = 0; k < 5; k++)
            {
                ros::Rate loop_rate(publish_frequency);  // 控制发布频率
                ros::Time start_time = ros::Time::now(); // 记录开始时间
                while (ros::ok() && ros::Time::now().toSec() - start_time.toSec() <= publish_duration)
                {
                    uint8_t head_1 = 0xAA;
                    uint8_t head_2 = 0X55;
                    uint8_t speed_low = getLowByte(speed);
                    uint8_t speed_next_low = getnext_LowByte(speed);
                    uint8_t speed_next_high = getnext_HighByte(speed);
                    uint8_t speed_high = getHighByte(speed);
                    uint8_t amplitude_low = getLowByte(amplitude);
                    uint8_t amplitude_next_low = getnext_LowByte(amplitude);
                    uint8_t amplitude_next_high = getnext_HighByte(amplitude);
                    uint8_t amplitude_high = getHighByte(amplitude);
                    uint8_t phase_low = getLowByte(phase);
                    uint8_t phase_next_low = getnext_LowByte(phase);
                    uint8_t phase_next_high = getnext_HighByte(phase);
                    uint8_t phase_high = getHighByte(phase);
                    uint8_t number[15] = {
                        head_1,
                        head_2,
                        speed_low,
                        speed_next_low,
                        speed_next_high,
                        speed_high,
                        amplitude_low,
                        amplitude_next_low,
                        amplitude_next_high,
                        amplitude_high,
                        phase_low,
                        phase_next_low,
                        phase_next_high,
                        phase_high,
                    };
                    uint8_t crcCode = crc8(number, 14);
                    number[14] = crcCode;
                    // 发送速度和时间指令给STM32
                    if (ser.isOpen())
                    {
                        ser.write(number, sizeof(number)); // 发送
                    }

                    loop_rate.sleep(); // 控制发布频率
                }
                phase += 20;
            }
            amplitude += 20;
        }
        speed += 100;
    }
}

// 手动输入
void task::manual()
{
    int speed_m = 0;
    int amplitude_m = 0;
    int phase_m = 0;
    int publish_frequency_m = 1; // 1000 Hz (1秒发布1000次)
    int publish_duration_m = 4;   
    ROS_INFO_STREAM("Please enter the rotational speed using the keyboard");
    cin >> speed_m;
    ROS_INFO_STREAM("Please enter the amplitude using the keyboard");
    cin >> amplitude_m;
    ROS_INFO_STREAM("Please enter the phase using the keyboard");
    cin >> phase_m;
    // ROS_INFO_STREAM("Please enter the frequency using the keyboard");
    // cin >> publish_frequency_m; // 发布频率
    // ROS_INFO_STREAM("Please enter the duration using the keyboard");
    // cin >> publish_duration_m;                 // 发布时长
    // publish_frequency_m = 1;                   // 发布频率
    // publish_duration_m = 1;                    // 发布时长
    ros::Rate loop_rate(publish_frequency_m);  // 控制发布频率
    ros::Time start_time_m = ros::Time::now(); // 记录开始时间
    while (ros::ok() && ros::Time::now().toSec() - start_time_m.toSec() <= publish_duration_m)
    {

        uint8_t head_1_m = 0xAA;
        uint8_t head_2_m = 0X55;
        uint8_t speed_low_m = getLowByte(speed_m);
        uint8_t speed_next_low_m = getnext_LowByte(speed_m);
        uint8_t speed_next_high_m = getnext_HighByte(speed_m);
        uint8_t speed_high_m = getHighByte(speed_m);
        uint8_t amplitude_low_m = getLowByte(amplitude_m);
        uint8_t amplitude_next_low_m = getnext_LowByte(amplitude_m);
        uint8_t amplitude_next_high_m = getnext_HighByte(amplitude_m);
        uint8_t amplitude_high_m = getHighByte(amplitude_m);
        uint8_t phase_low_m = getLowByte(phase_m);
        uint8_t phase_next_low_m = getnext_LowByte(phase_m);
        uint8_t phase_next_high_m = getnext_HighByte(phase_m);
        uint8_t phase_high_m = getHighByte(phase_m);
        uint8_t number[15] = {
            head_1_m,
            head_2_m,
            speed_low_m,
            speed_next_low_m,
            speed_next_high_m,
            speed_high_m,
            amplitude_low_m,
            amplitude_next_low_m,
            amplitude_next_high_m,
            amplitude_high_m,
            phase_low_m,
            phase_next_low_m,
            phase_next_high_m,
            phase_high_m,
        };
        uint8_t crcCode_m = crc8(number, 14);
        number[14] = crcCode_m;
        // 发送速度和时间指令给STM32
        if (ser.isOpen())
        {
            ser.write(number, sizeof(number)); // 发送
        }

        loop_rate.sleep(); // 控制发布频率
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "motor_control_node");
    ros::NodeHandle nh;
    task mytask;
    // 配置串口
    ser.setPort("/dev/ttyUSB0");
    ser.setBaudrate(115200);
    serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    ser.setTimeout(to);
    ser.open();

    if (!ser.isOpen())
    {
        ROS_ERROR("Failed to open serial port");
        return 1;
    }
    ros::Rate loop_rate(10);
    while (ros::ok && ser.isOpen())
    {
        ros::spinOnce();
        loop_rate.sleep();
        std::cout << "Intput key to chang state (or press ESC to quit)"
                  << std::endl;
        std::cout << "**************** taskState_************" << std::endl;
        std::cout << "      \"shift + A\": taskState_ is \"AUTOMATIC\""
                  << std::endl;
        std::cout << "      \"shift + M\": taskState_ is \"MANUAL\""
                  << std::endl;
        std::cout << "=======================================" << std::endl;

        keyValue = getch();

        switch (keyValue)
        {
        case A_ASCII_VALUE:
            ROS_INFO_STREAM("taskState_: \"automatic\"");
            mytask.automatic();
            break;
        case M_ASCII_VALUE:
            ROS_INFO_STREAM("taskState_: \"manual\"");
            mytask.manual();
            break;
        default:
            break;
        }
        if (keyValue == ESC_ASCII_VALUE)
        {
            break;
        }
    }

    ser.close();
    return 0;
}